"""
hangwrite gRPC Adapter - 简化版本
直接实现退化策略，不依赖 fsm_defense_agent

策略说明：
1. staging 阶段：无人船前往指定点并静止，无人机飞往指定航点后返回
2. defensive_degraded 阶段：无人机周期性起飞扫描右侧区域，无人船锁定最近目标并保持35km距离

作者: 何逸凡
版本: 3.0 - 简化版
"""

import sys
import grpc
import json
import math
import time
import re
from typing import List, Dict, Optional, Tuple
import numpy as np

# 将项目根目录添加到Python路径
sys.path.insert(0, sys.path[0]+"/../")

# 仿真系统通信协议
from sh15.simulation import simserver_pb2
from sh15.simulation import simserver_pb2_grpc

# 拦截算法
from sh15.algorithm.predator_prey_capture import compute_capture


# ==================== 简单数据类 ====================

class SimpleBoat:
    """简化的无人艇数据类"""
    def __init__(self, id: int, name: str, x: float, y: float, active: bool, 
                 heading: float = 0.0, speed: float = 0.0, max_speed: float = 10.0):
        self.id = id
        self.name = name
        self.x = x
        self.y = y
        self.active = active
        self.heading = heading
        self.speed = speed
        self.max_speed = max_speed


class SimpleDrone:
    """简化的无人机数据类"""
    def __init__(self, id: int, name: str, x: float, y: float, active: bool,
                 heading: float = 0.0, speed: float = 0.0, 
                 docked_on_boat_id: Optional[int] = -1,
                 parent_boat_id: Optional[int] = -1,
                 max_speed: float = 150.0, min_speed: float = 20.0,
                 home_name: Optional[str] = None):
        self.id = id
        self.name = name
        self.x = x
        self.y = y
        self.active = active
        self.heading = heading
        self.speed = speed
        self.docked_on_boat_id = docked_on_boat_id
        self.parent_boat_id = parent_boat_id
        self.max_speed = max_speed
        self.min_speed = min_speed
        self.home_name = home_name


class SimpleTarget:
    """简化的黑方目标数据类"""
    def __init__(self, id: int, name: str, x: float, y: float, active: bool):
        self.id = id
        self.name = name
        self.x = x
        self.y = y
        self.active = active


class SimpleObservation:
    """简化的观测数据类"""
    def __init__(self, white_boats: List[SimpleBoat], 
                 all_drones: List[SimpleDrone],
                 black_boats: List[SimpleTarget],
                 current_time: float):
        self.white_boats = white_boats
        self.all_drones = all_drones
        self.black_boats = black_boats
        self.current_time = current_time


# ==================== 工具函数 ====================

def grpc_course_to_heading(course_deg: float) -> float:
    """将gRPC航向(度,0=正北,顺时针)转换为数学角度(弧度,0=正东,逆时针)"""
    math_deg = (90 - course_deg) % 360
    return math.radians(math_deg)


def heading_to_grpc_course(heading_rad: float) -> float:
    """将数学角度(弧度,0=正东,逆时针)转换为gRPC航向(度,0=正北,顺时针)"""
    math_deg = math.degrees(heading_rad)
    grpc_deg = (90 - math_deg) % 360
    return grpc_deg


def parse_unit_id(unit_name: str) -> int:
    """从单位名称中解析出数字ID"""
    if not unit_name:
        return -1
    match = re.search(r'\d+', unit_name)
    return int(match.group()) if match else -1


# ==================== 主适配器类 ====================

class HangwriteGrpcAdapter:
    """简化版 gRPC 适配器 - 仅实现退化策略"""
    
    def __init__(self, host='127.0.0.1', port='6000', user_name='admin'):
        """初始化适配器"""
        # gRPC客户端设置
        self.channel = grpc.insecure_channel(f"{host}:{port}")
        self.client = simserver_pb2_grpc.GreeterStub(channel=self.channel)
        self.user_name = user_name
        print("gRPC客户端已连接。")
        
        # 单位名称映射（0基索引 -> 名称）
        self.white_boat_id_map: Dict[int, str] = {}
        self.white_drone_id_map: Dict[int, str] = {}
        self.black_boat_id_map: Dict[int, str] = {}
        
        # 时间戳
        self.last_update_time: float = 0.0
        
        # 缓存无人机母船ID映射（0基）
        self.drone_parent_boat_map: Dict[int, int] = {}
        
        # 退化策略：阶段管理与参数
        self._mode: str = 'staging'  # 'staging' -> 'defensive_degraded'
        
        # 给定点和航点（可外部配置）
        self.degrade_boat_targets: Dict[int, Tuple[float, float]] = {}
        self.degrade_drone_waypoint: Dict[int, Optional[Tuple[float, float]]] = {}
        
        # 阶段状态记录
        self._boat_reached: Dict[int, bool] = {}
        self._drone_stage_done: Dict[int, bool] = {}
        
        # 防御阶段：无人机周期扫视控制
        self._last_scan_time: float = 0.0
        self.scan_interval_s: float = 300.0  # 扫描间隔（秒）
        # 无人机扫描状态：'idle', 'scan_right_down', 'scan_right_up', 'returning'
        self._drone_scan_state: Dict[int, str] = {}
        
        # 35km 保距（米）
        self.standoff_distance_m: float = 35000.0
        
        # 60km y坐标过滤范围（米）
        self.y_filter_range_m: float = 60000.0
        
        # 初始化默认航点配置
        self._init_default_waypoints()
    
    # ==================== 航点配置 ====================
    
    def _init_default_waypoints(self):
        """
        初始化默认航点配置
        
        无人船（white_usv1-5，从北到南，id: 0-4）：
        - 所有 x = 100km = 100000m
        - 相距 120km = 120000m
        - 最中间（white_usv3，id=2）y = 673205/2 ≈ 336602.5m
        
        无人机（white_uav1-5，id: 0-4）：
        - 所有 x = 250km = 250000m
        - 最中间（white_uav3，id=2）水平向右飞（航向角0°）
        - 其余分布：±25°、±50°
        """
        # 计算无人船航点（从北到南）
        center_y = 673205.0 / 2.0  # 336602.5
        boat_x = 100000.0
        boat_spacing = 120000.0
        
        # white_usv1-5 对应 id 0-4
        boat_y_positions = [
            center_y + boat_spacing,      # id=0: white_usv1 (最北)
            center_y + boat_spacing / 2,  # id=1: white_usv2
            center_y,                      # id=2: white_usv3 (中间)
            center_y - boat_spacing / 2,  # id=3: white_usv4
            center_y - boat_spacing       # id=4: white_usv5 (最南)
        ]
        
        for boat_id in range(5):
            self.degrade_boat_targets[boat_id] = (boat_x, boat_y_positions[boat_id])
        
        # 计算无人机航点
        # 从母船(100000, y_usv)到目标点(250000, y_target)的航向角
        # dx = 250000 - 100000 = 150000
        # 对于航向角θ: dy = dx * tan(θ)
        drone_x = 250000.0
        dx_base = drone_x - boat_x  # 150000
        
        # 航向角配置（度）：+50°, +25°, 0°, -25°, -50°
        heading_angles_deg = [50.0, 25.0, 0.0, -25.0, -50.0]
        
        for drone_id in range(5):
            # 对应的母船y坐标
            usv_y = boat_y_positions[drone_id]
            # 航向角（转为弧度）
            heading_rad = math.radians(heading_angles_deg[drone_id])
            # 计算y偏移：dy = dx * tan(θ)
            dy = dx_base * math.tan(heading_rad)
            # 目标点y坐标
            target_y = usv_y + dy
            
            self.degrade_drone_waypoint[drone_id] = (drone_x, target_y)
        
        print("[配置] 无人船目标航点:")
        for boat_id in range(5):
            x, y = self.degrade_boat_targets[boat_id]
            print(f"  white_usv{boat_id+1} (id={boat_id}): ({x:.1f}m, {y:.1f}m)")
        
        print("[配置] 无人机目标航点:")
        for drone_id in range(5):
            waypoint = self.degrade_drone_waypoint.get(drone_id)
            if waypoint:
                x, y = waypoint
                heading = heading_angles_deg[drone_id]
                print(f"  white_uav{drone_id+1} (id={drone_id}): ({x:.1f}m, {y:.1f}m), 航向角: {heading:+.0f}°")
    
    # ==================== gRPC通信 ====================
    
    def _send_grpc_request(self, func_name: str, **kwargs) -> Optional[Dict]:
        """发送gRPC请求并返回JSON解析后的结果"""
        try:
            msg = json.dumps({
                "source": "hangwrite",
                "ip": "localhost",
                "user_name": self.user_name,
                "engine_name": "",
                "flag": "simulation",
                "func_name": func_name,
                "kwargs": kwargs or {}
            })
            request = self.client.control(simserver_pb2.MsgStr(msg=msg))  # type: ignore
            if request.data:
                return json.loads(request.data)
            if "success" in request.msg or "True" in request.msg:
                return {"status": "success"}
            return None
        except grpc.RpcError as e:
            print(f"gRPC请求失败: {func_name}, 错误: {e}")
            return None
        except json.JSONDecodeError as e:
            print(f"解析gRPC响应失败: {func_name}, 错误: {e}")
            return None
    
    # ==================== 主循环 ====================
    
    def run(self):
        """运行主循环"""
        print("启动退化策略决策循环...")
        while True:
            # 1. 获取仿真状态
            state = self._send_grpc_request("get_state")
            if not state:
                time.sleep(0.1)
                print("获取状态失败，等待0.1秒后重试...")
                continue
            
            # 2. 解析状态
            observation = self._parse_state(state)
            
            # 3. 生成并执行命令
            self._execute_degraded_strategy(observation)
    
    # ==================== 状态解析 ====================
    
    def _parse_state(self, state: Dict) -> SimpleObservation:
        """解析状态为简化的观测对象"""
        current_time = state.get("time", 0.0)
        
        # 解析白方无人艇
        white_boats = self._parse_white_boats(state.get("white_usv_states", []))
        
        # 解析白方无人机
        white_drones = self._parse_white_drones(state.get("white_uav_states", []))
        
        # 解析黑方目标
        black_boats = self._parse_black_boats(state.get("black_usv_states", []))
        
        return SimpleObservation(white_boats, white_drones, black_boats, current_time)
    
    def _parse_white_boats(self, white_usv_states: List[Dict]) -> List[SimpleBoat]:
        """解析白方无人艇"""
        boats = []
        for s in white_usv_states:
            name = s.get("name", "")
            uid = parse_unit_id(name)
            if uid == -1:
                continue
            
            id_zero_based = max(0, uid - 1)
            self.white_boat_id_map[id_zero_based] = name
            
            is_alive = bool(s.get("is_alive", False))
            pos = s.get("position") or [0.0, 0.0]
            x = float(pos[0]) if len(pos) > 0 else 0.0
            y = float(pos[1]) if len(pos) > 1 else 0.0
            
            if is_alive:
                speed = float(s.get("speed", 0.0))
                course = float(s.get("course", 0.0))
                heading = grpc_course_to_heading(course)
            else:
                speed = 0.0
                heading = 0.0
            
            boats.append(SimpleBoat(
                id=id_zero_based,
                name=name,
                x=x,
                y=y,
                active=is_alive,
                heading=heading,
                speed=speed
            ))
        
        return boats
    
    def _parse_white_drones(self, white_uav_states: List[Dict]) -> List[SimpleDrone]:
        """解析白方无人机"""
        drones = []
        for s in white_uav_states:
            name = s.get("name", "")
            uid = parse_unit_id(name)
            if uid == -1:
                continue
            
            id_zero_based = max(0, uid - 1)
            self.white_drone_id_map[id_zero_based] = name
            
            is_alive = bool(s.get("is_alive", False))
            pos = s.get("position") or [0.0, 0.0]
            x = float(pos[0]) if len(pos) > 0 else 0.0
            y = float(pos[1]) if len(pos) > 1 else 0.0
            
            if is_alive:
                speed = float(s.get("speed", 0.0))
                course = float(s.get("course", 0.0))
                heading = grpc_course_to_heading(course)
                is_at_usv = bool(s.get("is_at_usv", False))
                home_name = s.get("home_name")
            else:
                speed = 0.0
                heading = 0.0
                is_at_usv = False
                home_name = None
            
            # 确定停靠状态和母船ID
            docked_id = -1
            if is_at_usv and home_name:
                home_uid = parse_unit_id(home_name)
                docked_id = max(0, home_uid - 1) if home_uid != -1 else -1
                if docked_id != -1:
                    self.drone_parent_boat_map[id_zero_based] = docked_id
            
            parent_id = self.drone_parent_boat_map.get(id_zero_based, -1)
            if parent_id == -1 and docked_id >= 0:
                parent_id = docked_id
            
            drones.append(SimpleDrone(
                id=id_zero_based,
                name=name,
                x=x,
                y=y,
                active=is_alive,
                heading=heading,
                speed=speed,
                docked_on_boat_id=docked_id,
                parent_boat_id=parent_id,
                home_name=home_name
            ))
        
        return drones
    
    def _parse_black_boats(self, black_usv_states: List[Dict]) -> List[SimpleTarget]:
        """解析黑方目标（仅从状态列表，无视野信息）"""
        targets = []
        for s in black_usv_states:
            name = s.get("name", "")
            uid = parse_unit_id(name)
            if uid == -1:
                continue
            
            id_zero_based = max(0, uid - 1)
            self.black_boat_id_map[id_zero_based] = name
            
            is_alive = bool(s.get("is_alive", False))
            
            # 注意：非存活时 position 可能缺失
            # 这里我们需要从 white_observation 获取位置
            # 暂时设为 (0, 0)，在后续从 observation 补充
            targets.append(SimpleTarget(
                id=id_zero_based,
                name=name,
                x=0.0,
                y=0.0,
                active=is_alive
            ))
        
        return targets
    
    # ==================== 退化策略 ====================
    
    def _init_degraded_defaults(self, observation: SimpleObservation) -> None:
        """初始化默认目标点"""
        if not self.degrade_boat_targets:
            for b in observation.white_boats:
                self.degrade_boat_targets[b.id] = (float(b.x), float(b.y))
                self._boat_reached[b.id] = False
        
        if not self._drone_stage_done:
            for d in observation.all_drones:
                self._drone_stage_done[d.id] = False
                if d.id not in self.degrade_drone_waypoint:
                    self.degrade_drone_waypoint[d.id] = None
    
    def _execute_degraded_strategy(self, observation: SimpleObservation):
        """执行退化策略"""
        self._init_degraded_defaults(observation)
        
        if self._mode == 'staging':
            self._execute_staging(observation)
            # 检查是否应切换到 defensive_degraded
            if self._all_boats_reached() and self._all_drones_stage_done_and_docked(observation):
                self._mode = 'defensive_degraded'
                self._last_scan_time = observation.current_time
                print(f"[策略] 切换到 defensive_degraded 阶段，时间: {observation.current_time}")
        else:  # defensive_degraded
            self._execute_defensive_degraded(observation)
    
    def _all_boats_reached(self) -> bool:
        """检查所有船只是否到达"""
        return all(self._boat_reached.values()) if self._boat_reached else True
    
    def _all_drones_stage_done_and_docked(self, observation: SimpleObservation) -> bool:
        """检查所有无人机是否完成staging并停靠"""
        if not self._drone_stage_done:
            return True
        for d in observation.all_drones:
            if not self._drone_stage_done.get(d.id, False):
                return False
            if d.docked_on_boat_id is None or d.docked_on_boat_id < 0:
                return False
        return True
    
    # ==================== Staging 阶段 ====================
    
    def _execute_staging(self, observation: SimpleObservation):
        """执行 staging 阶段"""
        # 处理无人艇
        for b in observation.white_boats:
            if not b.active:
                continue
            self._staging_boat_action(b)
        
        # 处理无人机
        for d in observation.all_drones:
            if not d.active:
                continue
            self._staging_drone_action(d, observation)
    
    def _staging_boat_action(self, boat: SimpleBoat):
        """staging 阶段无人艇动作：前往指定点并静止"""
        target = self.degrade_boat_targets.get(boat.id)
        if not target:
            # 无目标：保持静止
            self._send_grpc_request("send_command", cmd={
                "unit_name": boat.name,
                "target_speed": 0.0,
                "target_course": heading_to_grpc_course(boat.heading)
            })
            return
        
        tx, ty = target
        dx, dy = tx - boat.x, ty - boat.y
        dist = math.hypot(dx, dy)
        
        if dist <= 200.0:
            # 到达，标记并静止
            self._boat_reached[boat.id] = True
            self._send_grpc_request("send_command", cmd={
                "unit_name": boat.name,
                "target_speed": 0.0,
                "target_course": heading_to_grpc_course(boat.heading)
            })
        else:
            # 前往目标点
            hd = math.atan2(dy, dx)
            self._send_grpc_request("send_command", cmd={
                "unit_name": boat.name,
                "target_speed": float(boat.max_speed),
                "target_course": heading_to_grpc_course(hd)
            })
    
    def _staging_drone_action(self, drone: SimpleDrone, observation: SimpleObservation):
        """staging 阶段无人机动作：飞往航点后返航"""
        wp = self.degrade_drone_waypoint.get(drone.id)
        
        # 无航点：直接标记完成
        if wp is None:
            self._drone_stage_done[drone.id] = True
            if drone.docked_on_boat_id is not None and drone.docked_on_boat_id >= 0:
                # 保持停靠
                pass
            else:
                # 返航降落
                self._send_drone_land_command(drone)
            return
        
        tx, ty = wp
        
        # 已完成且停靠：保持
        if self._drone_stage_done.get(drone.id, False):
            if drone.docked_on_boat_id is not None and drone.docked_on_boat_id >= 0:
                return
            else:
                # 应该在降落，继续降落
                self._send_drone_land_command(drone)
            return
        
        # 尚未完成
        if drone.docked_on_boat_id is not None and drone.docked_on_boat_id >= 0:
            # 起飞前往航点
            dx, dy = tx - drone.x, ty - drone.y
            hd = math.atan2(dy, dx)
            self._send_drone_takeoff_command(drone, drone.max_speed * 0.9, hd)
        else:
            # 在空中：飞向航点
            dx, dy = tx - drone.x, ty - drone.y
            dist = math.hypot(dx, dy)
            hd = math.atan2(dy, dx)
            
            if dist <= 500.0:
                # 到达航点，标记完成并返航
                self._drone_stage_done[drone.id] = True
                self._send_drone_land_command(drone)
            else:
                # 继续飞向航点
                self._send_grpc_request("send_command", cmd={
                    "unit_name": drone.name,
                    "target_speed": float(drone.max_speed * 0.9),
                    "target_course": heading_to_grpc_course(hd)
                })
    
    # ==================== Defensive Degraded 阶段 ====================
    
    def _execute_defensive_degraded(self, observation: SimpleObservation):
        """执行 defensive_degraded 阶段"""
        # 处理无人艇：锁定最近目标并保持35km
        for b in observation.white_boats:
            if not b.active:
                continue
            self._defensive_boat_action(b, observation)
        
        # 处理无人机：周期性扫描右侧区域
        for d in observation.all_drones:
            if not d.active:
                continue
            self._defensive_drone_action(d, observation)
    
    def _defensive_boat_action(self, boat: SimpleBoat, observation: SimpleObservation):
        """defensive 阶段无人艇动作：拦截y坐标60km内的最近目标"""
        # 过滤目标：只考虑y坐标差距在60km内的活跃目标
        valid_targets = []
        for t in observation.black_boats:
            if not t.active:
                continue
            y_diff = abs(t.y - boat.y)
            if y_diff <= self.y_filter_range_m:
                valid_targets.append(t)
        
        if not valid_targets:
            # 无有效目标：保持静止
            self._send_grpc_request("send_command", cmd={
                "unit_name": boat.name,
                "target_speed": 0.0,
                "target_course": heading_to_grpc_course(boat.heading)
            })
            return
        
        # 选择最近的目标
        target = min(valid_targets, key=lambda t: math.hypot(t.x - boat.x, t.y - boat.y))
        
        # 锁定目标
        self._send_grpc_request("cmd_lock", unit1_name=boat.name, unit2_name=target.name)
        
        # 计算拦截点
        intercept_result = self._calculate_intercept_point(
            boat, (target.x, target.y), 
            getattr(target, 'speed', 0.0),
            getattr(target, 'heading', 0.0)
        )
        
        if intercept_result is None:
            # 无法拦截，保持静止
            self._send_grpc_request("send_command", cmd={
                "unit_name": boat.name,
                "target_speed": 0.0,
                "target_course": heading_to_grpc_course(boat.heading)
            })
            return
        
        # 前往拦截点
        intercept_point, intercept_time, _ = intercept_result
        dx = intercept_point[0] - boat.x
        dy = intercept_point[1] - boat.y
        heading = math.atan2(dy, dx)
        
        self._send_grpc_request("send_command", cmd={
            "unit_name": boat.name,
            "target_speed": float(boat.max_speed),
            "target_course": heading_to_grpc_course(heading)
        })
    
    def _defensive_drone_action(self, drone: SimpleDrone, observation: SimpleObservation):
        """defensive 阶段无人机动作：右下→右上→返航循环扫描"""
        now = observation.current_time
        
        # 找母船
        parent = self._find_parent_boat(drone, observation)
        if parent is None:
            return
        
        # 初始化无人机扫描状态
        if drone.id not in self._drone_scan_state:
            self._drone_scan_state[drone.id] = 'idle'
        
        state = self._drone_scan_state[drone.id]
        
        # 停靠中：检查是否应该开始新的扫描周期
        if drone.docked_on_boat_id is not None and drone.docked_on_boat_id >= 0:
            if state == 'idle' and (now - self._last_scan_time) >= self.scan_interval_s:
                # 开始新的扫描周期：起飞前往右下
                self._drone_scan_state[drone.id] = 'scan_right_down'
                scan_dx, scan_dy = 4000.0, -4000.0  # 右下偏移
                tx, ty = parent.x + scan_dx, parent.y + scan_dy
                hd = math.atan2(ty - drone.y, tx - drone.x)
                self._send_drone_takeoff_command(drone, drone.max_speed , hd)
            # 其他情况保持停靠
            return
        
        # 在空中：根据状态执行不同行为
        if state == 'scan_right_down':
            # 飞往右下区域
            scan_dx, scan_dy = 4000.0, -4000.0
            tx, ty = parent.x + scan_dx, parent.y + scan_dy
            dx, dy = tx - drone.x, ty - drone.y
            dist = math.hypot(dx, dy)
            
            if dist <= 800.0:
                # 到达右下，转向右上
                self._drone_scan_state[drone.id] = 'scan_right_up'
                scan_dx, scan_dy = 4000.0, 4000.0  # 右上偏移
                tx, ty = parent.x + scan_dx, parent.y + scan_dy
            
            hd = math.atan2(ty - drone.y, tx - drone.x)
            self._send_grpc_request("send_command", cmd={
                "unit_name": drone.name,
                "target_speed": float(drone.max_speed ),
                "target_course": heading_to_grpc_course(hd)
            })
        
        elif state == 'scan_right_up':
            # 飞往右上区域
            scan_dx, scan_dy = 4000.0, 4000.0
            tx, ty = parent.x + scan_dx, parent.y + scan_dy
            dx, dy = tx - drone.x, ty - drone.y
            dist = math.hypot(dx, dy)
            
            if dist <= 800.0:
                # 到达右上，返航
                self._drone_scan_state[drone.id] = 'returning'
                self._last_scan_time = now  # 刷新扫描时间
                self._send_drone_land_command(drone, parent)
            else:
                hd = math.atan2(dy, dx)
                self._send_grpc_request("send_command", cmd={
                    "unit_name": drone.name,
                    "target_speed": float(drone.max_speed ),
                    "target_course": heading_to_grpc_course(hd)
                })
        
        elif state == 'returning':
            # 返航降落
            self._send_drone_land_command(drone, parent)
            # 降落成功后会自动变为停靠状态，重置为idle
            if drone.docked_on_boat_id is not None and drone.docked_on_boat_id >= 0:
                self._drone_scan_state[drone.id] = 'idle'
    
    # ==================== 辅助函数 ====================
    
    def _calculate_intercept_point(self, boat: SimpleBoat, target_pos: Tuple[float, float], 
                                  target_speed: float, target_heading: float) -> Optional[Tuple[Tuple[float, float], float, Tuple[float, float]]]:
        """计算拦截点（考虑40km锁定距离）
        
        Args:
            boat: 白方无人船
            target_pos: 目标位置 (x, y)
            target_speed: 目标速度 (m/s)
            target_heading: 目标航向 (弧度)
            
        Returns:
            ((intercept_x, intercept_y), intercept_time, (boat_x, boat_y)) 或 None
        """
        try:
            result = compute_capture(
                X0=float(boat.x), 
                Y0=float(boat.y),
                X1=float(target_pos[0]), 
                Y1=float(target_pos[1]),
                ux=float(math.cos(target_heading)), 
                uy=float(math.sin(target_heading)),
                R=40000.0,  # 40km 锁定距离
                vp=float(boat.max_speed),
                vq=float(target_speed)
            )
            
            # 解析结果
            if not isinstance(result, dict):
                return None
            
            capture_ok = bool(result.get("capture", False))
            if not capture_ok:
                return None
            
            intercept_time = result.get("t")
            prey_pos = result.get("prey_pos")
            
            if intercept_time is None or prey_pos is None:
                return None
            
            # 类型检查和转换
            if not isinstance(prey_pos, (list, tuple)) or len(prey_pos) < 2:
                return None
            
            t_val = float(str(intercept_time))
            px, py = float(str(prey_pos[0])), float(str(prey_pos[1]))
            
            # 计算无人船在拦截时的位置
            boat_x = float(boat.x + boat.max_speed * math.cos(boat.heading) * t_val)
            boat_y = float(boat.y + boat.max_speed * math.sin(boat.heading) * t_val)
            
            return ((px, py), t_val, (boat_x, boat_y))
            
        except Exception as e:
            print(f"[拦截点计算失败] {e}")
            return None
    
    def _find_parent_boat(self, drone: SimpleDrone, observation: SimpleObservation) -> Optional[SimpleBoat]:
        """查找无人机母船"""
        # 优先使用 parent_boat_id
        if drone.parent_boat_id is not None and drone.parent_boat_id >= 0:
            for b in observation.white_boats:
                if b.id == drone.parent_boat_id and b.active:
                    return b
        
        # 其次使用 docked_on_boat_id
        if drone.docked_on_boat_id is not None and drone.docked_on_boat_id >= 0:
            for b in observation.white_boats:
                if b.id == drone.docked_on_boat_id and b.active:
                    return b
        
        # 最后返回最近的活跃船只
        active_boats = [b for b in observation.white_boats if b.active]
        if not active_boats:
            return None
        return min(active_boats, key=lambda b: math.hypot(b.x - drone.x, b.y - drone.y))
    
    def _send_drone_takeoff_command(self, drone: SimpleDrone, speed: float, heading: float):
        """发送无人机起飞命令"""
        if not drone.home_name:
            return
        self._send_grpc_request("cmd_uav_takeoff",
            unit_name=drone.name,
            home_name=drone.home_name,
            target_speed=float(speed),
            target_course=heading_to_grpc_course(heading)
        )
    
    def _send_drone_land_command(self, drone: SimpleDrone, parent_boat: Optional[SimpleBoat] = None):
        """发送无人机降落命令
        
        降落规则：
        - 降落速度必须为 20 m/s
        - 必须在母船100m内才能降落成功
        - 提前减速：距离1000m开始减速，100m内保持20 m/s
        """
        if not drone.home_name:
            return
        
        if parent_boat is None:
            # 无法获取母船信息，直接发送降落命令
            self._send_grpc_request("cmd_uav_land",
                uav_name=drone.name,
                usv_name=drone.home_name
            )
            return
        
        # 计算与母船的距离
        dist = math.hypot(drone.x - parent_boat.x, drone.y - parent_boat.y)
        dx = parent_boat.x - drone.x
        dy = parent_boat.y - drone.y
        hd = math.atan2(dy, dx)
        
        # 根据距离调整速度
        if dist <= 100.0:
            # 100m内：保持20 m/s并发送降落命令
            speed = 20.0
            self._send_grpc_request("send_command", cmd={
                "unit_name": drone.name,
                "target_speed": speed,
                "target_course": heading_to_grpc_course(hd)
            })
            self._send_grpc_request("cmd_uav_land",
                uav_name=drone.name,
                usv_name=drone.home_name
            )
        elif dist <= 1000.0:
            # 100m-1000m：线性减速到20 m/s
            # 距离300m时速度=max_speed，距离100m时速度=20
            speed_range = drone.max_speed - 20.0
            dist_range = 1000.0 - 100.0
            speed = 20.0 + speed_range * (dist - 100.0) / dist_range
            self._send_grpc_request("send_command", cmd={
                "unit_name": drone.name,
                "target_speed": float(speed),
                "target_course": heading_to_grpc_course(hd)
            })
        else:
            # 距离 > 1000m：使用最大速度接近
            self._send_grpc_request("send_command", cmd={
                "unit_name": drone.name,
                "target_speed": float(drone.max_speed),
                "target_course": heading_to_grpc_course(hd)
            })


# ==================== 主程序入口 ====================

if __name__ == "__main__":
    adapter = HangwriteGrpcAdapter()
    try:
        adapter.run()
    except KeyboardInterrupt:
        print("\n用户中断程序。")
    except Exception as e:
        print(f"\n程序出现严重错误: {e}")
        import traceback
        traceback.print_exc()
    finally:
        print("程序退出。")
